// Memory layout utilities for GridCut algorithm
//
// This module provides functions for:
// - Dimension rounding for cache-aligned memory layout
// - Block-based node indexing for improved cache locality
// - Neighbor navigation functions for 2D and 3D grids

/// Rounds up to the next multiple of 8
/// Used for 2D grid dimension padding to align with 8x8 blocks
#[inline]
pub(crate) fn next_higher_mul8(n: usize) -> usize {
    (n + 7) & !7
}

/// Rounds up to the next multiple of 4
/// Used for 3D grid dimension padding to align with 4x4x4 blocks
#[inline]
pub(crate) fn next_higher_mul4(n: usize) -> usize {
    (n + 3) & !3
}

/// 2D block-based node indexing structure
/// Nodes are arranged in 8x8 blocks for cache locality
#[derive(Debug)]
pub(crate) struct NodeIndex2D {
    width_blocks: usize,
    y_offset: usize,
}

impl NodeIndex2D {
    /// Creates a new 2D node indexer
    /// 
    /// # Arguments
    /// * `padded_width` - Width rounded up to multiple of 8
    /// * `padded_height` - Height rounded up to multiple of 8
    pub(crate) fn new(padded_width: usize, padded_height: usize) -> Self {
        let width_blocks = padded_width >> 3;
        let _height_blocks = padded_height >> 3;
        let y_offset = (width_blocks << 6) - 56;
        
        Self {
            width_blocks,
            y_offset,
        }
    }
    
    /// Converts 2D coordinates to node ID using block-based indexing
    /// 
    /// Formula: ((x>>3) + (y>>3)*WB)<<6) + (x&7) + ((y&7)<<3)
    /// 
    /// # Arguments
    /// * `x` - X coordinate (0-based)
    /// * `y` - Y coordinate (0-based)
    #[inline]
    pub(crate) fn node_id(&self, x: usize, y: usize) -> usize {
        let block_x = x >> 3;
        let block_y = y >> 3;
        let in_block_x = x & 7;
        let in_block_y = y & 7;
        
        ((block_x + block_y * self.width_blocks) << 6) + in_block_x + (in_block_y << 3)
    }
    
    /// Navigate to left neighbor (LE: [-1, 0])
    /// Handles block boundaries correctly
    #[inline]
    pub(crate) fn neighbor_le(&self, v: usize) -> usize {
        if (v & 0x07) == 0 {
            v.wrapping_sub(57)  // Cross block boundary
        } else {
            v - 1   // Within block
        }
    }
    
    /// Navigate to right neighbor (GE: [+1, 0])
    /// Handles block boundaries correctly
    #[inline]
    pub(crate) fn neighbor_ge(&self, v: usize) -> usize {
        if (v & 0x07) == 0x07 {
            v + 57  // Cross block boundary
        } else {
            v + 1   // Within block
        }
    }
    
    /// Navigate to upper neighbor (EL: [0, -1])
    /// Handles block boundaries correctly
    #[inline]
    pub(crate) fn neighbor_el(&self, v: usize) -> usize {
        if (v & 0x38) == 0 {
            v.wrapping_sub(self.y_offset)  // Cross block boundary
        } else {
            v - 8              // Within block
        }
    }
    
    /// Navigate to lower neighbor (EG: [0, +1])
    /// Handles block boundaries correctly
    #[inline]
    pub(crate) fn neighbor_eg(&self, v: usize) -> usize {
        if (v & 0x38) == 0x38 {
            v + self.y_offset  // Cross block boundary
        } else {
            v + 8              // Within block
        }
    }
}

/// 3D block-based node indexing structure
/// Nodes are arranged in 4x4x4 blocks for cache locality
pub(crate) struct NodeIndex3D {
    width_blocks: usize,
    height_blocks: usize,
    xy_offset: usize,
    z_offset: usize,
}

impl NodeIndex3D {
    /// Creates a new 3D node indexer
    /// 
    /// # Arguments
    /// * `padded_width` - Width rounded up to multiple of 4
    /// * `padded_height` - Height rounded up to multiple of 4
    /// * `padded_depth` - Depth rounded up to multiple of 4
    pub(crate) fn new(padded_width: usize, padded_height: usize, padded_depth: usize) -> Self {
        let width_blocks = padded_width >> 2;
        let height_blocks = padded_height >> 2;
        let _depth_blocks = padded_depth >> 2;
        
        // xy_offset: distance to jump when crossing y block boundary
        // From end of one y-block to start of next: (width_blocks * 64) - 4*3 = (width_blocks * 64) - 12
        let xy_offset = (width_blocks << 6) - 12;
        
        // z_offset: distance to jump when crossing z block boundary  
        // From end of one z-block to start of next: (width_blocks * height_blocks * 64) - 16*3
        let z_offset = (width_blocks * height_blocks << 6) - 48;
        
        Self {
            width_blocks,
            height_blocks,
            xy_offset,
            z_offset,
        }
    }
    
    /// Converts 3D coordinates to node ID using block-based indexing
    /// 
    /// Formula: ((x>>2) + (y>>2)*WB + (z>>2)*WB*HB)<<6) + (x&3) + ((y&3)<<2) + ((z&3)<<4)
    /// 
    /// # Arguments
    /// * `x` - X coordinate (0-based)
    /// * `y` - Y coordinate (0-based)
    /// * `z` - Z coordinate (0-based)
    #[inline]
    pub(crate) fn node_id(&self, x: usize, y: usize, z: usize) -> usize {
        let block_x = x >> 2;
        let block_y = y >> 2;
        let block_z = z >> 2;
        let in_block_x = x & 3;
        let in_block_y = y & 3;
        let in_block_z = z & 3;
        
        ((block_x + block_y * self.width_blocks + block_z * self.width_blocks * self.height_blocks) << 6)
            + in_block_x
            + (in_block_y << 2)
            + (in_block_z << 4)
    }
    
    /// Navigate to left neighbor in X direction ([-1, 0, 0])
    #[inline]
    pub(crate) fn neighbor_x_minus(&self, v: usize) -> usize {
        if (v & 0x03) == 0 {
            v.wrapping_sub(61)  // Cross block boundary
        } else {
            v - 1   // Within block
        }
    }
    
    /// Navigate to right neighbor in X direction ([+1, 0, 0])
    #[inline]
    pub(crate) fn neighbor_x_plus(&self, v: usize) -> usize {
        if (v & 0x03) == 0x03 {
            v + 61  // Cross block boundary
        } else {
            v + 1   // Within block
        }
    }
    
    /// Navigate to neighbor in -Y direction ([0, -1, 0])
    #[inline]
    pub(crate) fn neighbor_y_minus(&self, v: usize) -> usize {
        if (v & 0x0C) == 0 {
            v.wrapping_sub(self.xy_offset)  // Cross block boundary
        } else {
            v - 4               // Within block
        }
    }
    
    /// Navigate to neighbor in +Y direction ([0, +1, 0])
    #[inline]
    pub(crate) fn neighbor_y_plus(&self, v: usize) -> usize {
        if (v & 0x0C) == 0x0C {
            v + self.xy_offset  // Cross block boundary
        } else {
            v + 4               // Within block
        }
    }
    
    /// Navigate to neighbor in -Z direction ([0, 0, -1])
    #[inline]
    pub(crate) fn neighbor_z_minus(&self, v: usize) -> usize {
        if (v & 0x30) == 0 {
            v.wrapping_sub(self.z_offset)  // Cross block boundary
        } else {
            v - 16             // Within block
        }
    }
    
    /// Navigate to neighbor in +Z direction ([0, 0, +1])
    #[inline]
    pub(crate) fn neighbor_z_plus(&self, v: usize) -> usize {
        if (v & 0x30) == 0x30 {
            v + self.z_offset  // Cross block boundary
        } else {
            v + 16             // Within block
        }
    }
}

#[cfg(test)]
mod tests {
    use super::*;
    
    #[test]
    fn test_next_higher_mul8() {
        assert_eq!(next_higher_mul8(0), 0);
        assert_eq!(next_higher_mul8(1), 8);
        assert_eq!(next_higher_mul8(7), 8);
        assert_eq!(next_higher_mul8(8), 8);
        assert_eq!(next_higher_mul8(9), 16);
        assert_eq!(next_higher_mul8(15), 16);
        assert_eq!(next_higher_mul8(16), 16);
        assert_eq!(next_higher_mul8(100), 104);
    }
    
    #[test]
    fn test_next_higher_mul4() {
        assert_eq!(next_higher_mul4(0), 0);
        assert_eq!(next_higher_mul4(1), 4);
        assert_eq!(next_higher_mul4(3), 4);
        assert_eq!(next_higher_mul4(4), 4);
        assert_eq!(next_higher_mul4(5), 8);
        assert_eq!(next_higher_mul4(7), 8);
        assert_eq!(next_higher_mul4(8), 8);
        assert_eq!(next_higher_mul4(100), 100);
    }
    
    #[test]
    fn test_2d_node_id_simple() {
        let indexer = NodeIndex2D::new(16, 16);
        
        // Test first block (0,0) to (7,7)
        assert_eq!(indexer.node_id(0, 0), 0);
        assert_eq!(indexer.node_id(1, 0), 1);
        assert_eq!(indexer.node_id(7, 0), 7);
        assert_eq!(indexer.node_id(0, 1), 8);
        assert_eq!(indexer.node_id(7, 7), 63);
        
        // Test second block in x direction (8,0) to (15,7)
        assert_eq!(indexer.node_id(8, 0), 64);
        assert_eq!(indexer.node_id(9, 0), 65);
        assert_eq!(indexer.node_id(15, 7), 127);
        
        // Test second block in y direction (0,8) to (7,15)
        assert_eq!(indexer.node_id(0, 8), 128);
        assert_eq!(indexer.node_id(7, 15), 191);
    }
    
    #[test]
    fn test_2d_neighbor_navigation_interior() {
        let indexer = NodeIndex2D::new(16, 16);
        
        // Test interior node (3, 3) - all neighbors within same block
        let node = indexer.node_id(3, 3);
        
        let left = indexer.neighbor_le(node);
        assert_eq!(left, indexer.node_id(2, 3));
        
        let right = indexer.neighbor_ge(node);
        assert_eq!(right, indexer.node_id(4, 3));
        
        let up = indexer.neighbor_el(node);
        assert_eq!(up, indexer.node_id(3, 2));
        
        let down = indexer.neighbor_eg(node);
        assert_eq!(down, indexer.node_id(3, 4));
    }
    
    #[test]
    fn test_2d_neighbor_navigation_block_boundary() {
        let indexer = NodeIndex2D::new(16, 16);
        
        // Test node at x block boundary (7, 3)
        let node = indexer.node_id(7, 3);
        
        let right = indexer.neighbor_ge(node);
        assert_eq!(right, indexer.node_id(8, 3));
        
        // Test node at y block boundary (3, 7)
        let node = indexer.node_id(3, 7);
        
        let down = indexer.neighbor_eg(node);
        assert_eq!(down, indexer.node_id(3, 8));
    }
    
    #[test]
    fn test_3d_node_id_simple() {
        let indexer = NodeIndex3D::new(8, 8, 8);
        
        // Test first block (0,0,0) to (3,3,3)
        assert_eq!(indexer.node_id(0, 0, 0), 0);
        assert_eq!(indexer.node_id(1, 0, 0), 1);
        assert_eq!(indexer.node_id(3, 0, 0), 3);
        assert_eq!(indexer.node_id(0, 1, 0), 4);
        assert_eq!(indexer.node_id(0, 0, 1), 16);
        assert_eq!(indexer.node_id(3, 3, 3), 63);
        
        // Test second block in x direction (4,0,0)
        assert_eq!(indexer.node_id(4, 0, 0), 64);
        
        // Test second block in y direction (0,4,0)
        assert_eq!(indexer.node_id(0, 4, 0), 128);
        
        // Test second block in z direction (0,0,4)
        assert_eq!(indexer.node_id(0, 0, 4), 256);
    }
    
    #[test]
    fn test_3d_neighbor_navigation_interior() {
        let indexer = NodeIndex3D::new(8, 8, 8);
        
        // Test interior node (2, 2, 2) - all neighbors within same block
        let node = indexer.node_id(2, 2, 2);
        
        let x_minus = indexer.neighbor_x_minus(node);
        assert_eq!(x_minus, indexer.node_id(1, 2, 2));
        
        let x_plus = indexer.neighbor_x_plus(node);
        assert_eq!(x_plus, indexer.node_id(3, 2, 2));
        
        let y_minus = indexer.neighbor_y_minus(node);
        assert_eq!(y_minus, indexer.node_id(2, 1, 2));
        
        let y_plus = indexer.neighbor_y_plus(node);
        assert_eq!(y_plus, indexer.node_id(2, 3, 2));
        
        let z_minus = indexer.neighbor_z_minus(node);
        assert_eq!(z_minus, indexer.node_id(2, 2, 1));
        
        let z_plus = indexer.neighbor_z_plus(node);
        assert_eq!(z_plus, indexer.node_id(2, 2, 3));
    }
    
    #[test]
    fn test_3d_neighbor_navigation_block_boundary() {
        let indexer = NodeIndex3D::new(8, 8, 8);
        
        // Test node at x block boundary (3, 2, 2)
        let node = indexer.node_id(3, 2, 2);
        let x_plus = indexer.neighbor_x_plus(node);
        assert_eq!(x_plus, indexer.node_id(4, 2, 2));
        
        // Test node at y block boundary (2, 3, 2)
        let node = indexer.node_id(2, 3, 2);
        let y_plus = indexer.neighbor_y_plus(node);
        assert_eq!(y_plus, indexer.node_id(2, 4, 2));
        
        // Test node at z block boundary (2, 2, 3)
        let node = indexer.node_id(2, 2, 3);
        let z_plus = indexer.neighbor_z_plus(node);
        assert_eq!(z_plus, indexer.node_id(2, 2, 4));
    }
}
